#include "pBody.h"
#include "Box2D.h"
#include "physics.h"
#include "pMaterial.h"

using namespace Tmpl8;

//Debug draw
extern void LineDraw(vector3 pos1, vector3 pos2, unsigned int color);
extern void CircleDraw(vector3 pos, float rad, float angle, unsigned int color);


pBody::pBody(unsigned int aIndex, vector3 aPos, float aAngle, PHYSICS_MODE aMode) : pEntity(aIndex)
{
	mMode = aMode;

	aPos = aPos * SCALE_IN;

	//Defining the body:
	b2BodyDef bdef = b2BodyDef();
	bdef.angle = aAngle;
	bdef.position = b2Vec2(aPos.x, aPos.y);
	bdef.angularDamping = 0.07;

	if		(aMode == PMODE_OFF)		{	bdef.active = false;	bdef.type = b2_dynamicBody;	}
	else if (aMode == PMODE_DYNAMIC)	{	bdef.type = b2_dynamicBody;							}
	else if (aMode == PMODE_KINEMATIC)	{	bdef.type = b2_kinematicBody;						}
	else if (aMode == PMODE_STATIC)		{	bdef.type = b2_staticBody;							}

	bdef.userData = this;
	mBody = mPhysics->GetWorld()->CreateBody(&bdef);
}

void pBody::AddBox(vector3 aPos, vector3 aSize, float aAngle, pMaterial* aMat, float weight)
{
	b2FixtureDef fdef = aMat->GetFixture();
	b2PolygonShape shape = b2PolygonShape();

	aPos = aPos * SCALE_IN;
	aSize = aSize * SCALE_IN;

	shape.SetAsBox(aSize.x/2, aSize.y /2, b2Vec2(aPos.x, aPos.y), aAngle);
	fdef.shape = &shape;
	fdef.userData = aMat;
	float surfaceSize = (aSize.x * aSize.x) + (aSize.y * aSize.y);
	fdef.density = weight/surfaceSize;
	mBody->CreateFixture(&fdef);
}

void pBody::AddCircle(vector3 aPos, float aRadius, pMaterial* aMat, float weight)
{
	b2FixtureDef fdef= aMat->GetFixture();
	b2CircleShape shape = b2CircleShape();

	aPos = aPos * SCALE_IN;
	aRadius = aRadius * SCALE_IN;

	shape.m_p.Set(aPos.x, aPos.y);
	shape.m_radius = aRadius;
	fdef.shape = &shape;
	fdef.userData = aMat;
	float surfaceSize = 2 * PI * (aRadius * aRadius);
	fdef.density = weight/surfaceSize;
	mBody->CreateFixture(&fdef);
}

void pBody::AddLine( vector3 aPos1, vector3 aPos2, pMaterial* aMat )
{
	b2FixtureDef fdef = aMat->GetFixture();
	b2PolygonShape shape = b2PolygonShape();

	aPos1 = aPos1 * SCALE_IN;
	aPos2 = aPos2 * SCALE_IN;

	shape.SetAsEdge(b2Vec2(aPos1.x, aPos1.y), b2Vec2(aPos2.x, aPos2.y));
	fdef.shape = &shape;
	
	fdef.userData = aMat;

	mBody->CreateFixture(&fdef);
}

void pBody::AddVertices(b2Vec2 *aVertices, unsigned int aNumberOfVertices, pMaterial* aMat, b2Vec2 aOffset )
{
	b2FixtureDef fdef = aMat->GetFixture();
	b2PolygonShape shape = b2PolygonShape();

	b2Vec2 CenterPoint = (aVertices[0] + aVertices[1] + aVertices[2]);
	CenterPoint.x /= 3.0f;
	CenterPoint.y /= 3.0f;

	for(unsigned int i = 0; i < aNumberOfVertices; i++)
	{
		aVertices[i] += aOffset;
		aVertices[i] -= CenterPoint;
		aVertices[i] *= 1.2f;
		aVertices[i] += CenterPoint;
		aVertices[i] *= SCALE_IN;
	}

	shape.Set(aVertices, aNumberOfVertices);
	fdef.shape = &shape;
	fdef.userData = aMat;
	mBody->CreateFixture(&fdef);
}


void pBody::Draw()
{
#ifdef _DEBUG
	b2Fixture* list = mBody->GetFixtureList();
	b2Vec2 bodyPos = mBody->GetPosition();
	float angle = mBody->GetAngle();
	float cosAngle = cos(angle);
	float sinAngle = sin(angle);

	for (; list != 0; list = list->GetNext())
	{
		b2Shape* shape = list->GetShape();
		if (shape->GetType() == b2Shape::Type::e_polygon)
		{
			b2PolygonShape* pshape = (b2PolygonShape*)shape;
			vector3 pos1;
			vector3 pos2;

			int count = pshape->GetVertexCount();
			for (int i = 0; i < count; i++)
			{
				b2Vec2 vex1 = pshape->GetVertex(i);
				b2Vec2 vex2 = pshape->GetVertex((i+1) % count);
				pos1.x = (vex1.x * cosAngle) - (vex1.y * sinAngle);
				pos1.y = (vex1.y * cosAngle) + (vex1.x * sinAngle);
				pos2.x = (vex2.x * cosAngle) - (vex2.y * sinAngle);
				pos2.y = (vex2.y * cosAngle) + (vex2.x * sinAngle);

				pos1.x = (pos1.x + bodyPos.x) * SCALE_OUT;
				pos1.y = (pos1.y + bodyPos.y) * SCALE_OUT;
				pos2.x = (pos2.x + bodyPos.x) * SCALE_OUT;
				pos2.y = (pos2.y + bodyPos.y) * SCALE_OUT;
				LineDraw(pos1 - *mPhysics->GetOffset(), pos2 - *mPhysics->GetOffset(), 0xffffff);
			}
		}
		else if (shape->GetType() == b2Shape::Type::e_circle)
		{
			b2CircleShape* cshape = (b2CircleShape*)shape;
			vector3 pos;

			float radius = cshape->m_radius * SCALE_OUT;
			b2Vec2 vec = cshape->m_p;

			pos.x = (vec.x * cosAngle) - (vec.y * sinAngle);
			pos.y = (vec.y * cosAngle) + (vec.x * sinAngle);
			pos.x = (pos.x + bodyPos.x) * SCALE_OUT;
			pos.y = (pos.y + bodyPos.y) * SCALE_OUT;

			CircleDraw(pos - *mPhysics->GetOffset(), radius, mBody->GetAngle(), 0xffffff );
		}
	}	
#endif
}

void pBody::Tick()
{

}

void pBody::SetPosition(vector3 aPos)
{
	mBody->SetTransform(b2Vec2(aPos.x, aPos.y), mBody->GetAngle());
}

vector3 pBody::GetPosition()
{
	b2Vec2 result = mBody->GetPosition();
	return vector3(result.x, result.y, 0);
}

void pBody::SetAngle(float aAngle)
{
	mBody->SetTransform(mBody->GetPosition(), aAngle);
}

float pBody::GetAngle()
{
	return mBody->GetAngle();
}

void Tmpl8::pBody::Remove2DBoxBody()
{
	mPhysics->GetWorld()->DestroyBody(mBody);
}